/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Wall Following
 *         $Id: wall.c 26 2014-04-13 22:56:33Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdio.h>
#include <string.h>
#include "maxq_config.h"
#include "comm/maxq_comm.h"
#include "car.h"
#include "wall.h"
#include "dist.h"

/***** Definitions *****/

/***** Global Data *****/
int wall_dist[NUM_WALLS];
int wall_lock[NUM_WALLS];

/***** File Scope Data *****/
static int wall_alarm_type;
static void (*wall_callback)(void);
static int last_valid_dist[NUM_WALLS];
static int invalid_cnt[NUM_WALLS];

/***** Function Prototypes *****/

/******************************************************************************/
void wall_init(void)
{
  int wall;

  wall_alarm_type = WALL_NONE;
  wall_callback = NULL;

  for (wall = 0; wall < NUM_WALLS; wall++) {
    wall_dist[wall] = WALL_INVALID;
    wall_lock[wall] = 0;
    invalid_cnt[wall] = 0;
  }
}

/******************************************************************************/
void wall_task(int expL, int expR)
{
  int newDist[NUM_WALLS] = { WALL_INVALID, WALL_INVALID };
  int expDist[NUM_WALLS] = { expL, expR };
  int wall;
  int expWidth = WALL_INVALID;
  int newWidth = WALL_INVALID;
  int errorWidth = WALL_INVALID;
  int errorExpDist, errorLastDist;
  void (*temp_callback)(void);

  // Get the new measured distance
  newDist[LEFT] = distance[LEFT];
  newDist[RIGHT] = distance[RIGHT];

  // Calculate expected and measured hallway width if possible
  if ( (expL > 0) && (expR > 0) ) {
    expWidth = expL + expR + DIST_BETWEEN_SENSORS;

    if ( (distance[LEFT] > 0) && (distance[RIGHT] > 0) ) {
      newWidth = distance[LEFT] + distance[RIGHT] + DIST_BETWEEN_SENSORS;
      if (newWidth > expWidth)
        errorWidth = newWidth - expWidth;
      else
        errorWidth = expWidth - newWidth;
    }
  }

  // Invalidate lock if we don't expect a wall
  if (expL < 0)
    wall_lock[LEFT] = 0;
  if (expR < 0)
    wall_lock[RIGHT] = 0;

  // Try to validate wall distances
  if ( (errorWidth != WALL_INVALID) && (errorWidth < (expWidth >> 3)) ) {
    wall_lock[LEFT] = 1;
    wall_lock[RIGHT] = 1;
    invalid_cnt[LEFT] = 0;
    invalid_cnt[RIGHT] = 0;
  }
  else {
    for (wall = 0; wall < NUM_WALLS; wall++) {

      if (expDist[wall] == WALL_INVALID) {
        wall_lock[wall] = 0;
        wall_dist[wall] = WALL_INVALID;
      }
      else {
        // Calculate the difference from the expected wall distance
        if (expDist[wall] > distance[wall])
          errorExpDist = expDist[wall] - distance[wall];
        else
          errorExpDist = distance[wall] - expDist[wall];
  
        // Calculate the difference from the last valid wall distance
        if ( (last_valid_dist[wall] != WALL_INVALID) && (distance[wall] != WALL_INVALID) ) {
          if (last_valid_dist[wall] > distance[wall])
            errorLastDist = last_valid_dist[wall] - distance[wall];
          else
            errorLastDist = distance[wall] - last_valid_dist[wall];
        } else {
          errorLastDist = WALL_INVALID;
        }
  
        if (errorExpDist < (expDist[wall] >> 2)) {
          // We are close to the expected distance, it is valid
          wall_lock[wall] = 1;
          invalid_cnt[wall] = 0;
        }
        else if ( (errorLastDist != WALL_INVALID) && (errorLastDist < (last_valid_dist[wall] >> 3)) ) {
          // We are close to the last valid distance, it is valid
          wall_lock[wall] = 1;
          invalid_cnt[wall] = 0;
        }
        else {
          // This is not a valid wall distance
          if (invalid_cnt[wall] < 2) {
            invalid_cnt[wall]++;
            newDist[wall] = last_valid_dist[wall];
          } else {
            wall_lock[wall] = 0;
            // Is this a wall opening
            if (distance[wall] > (expDist[wall] + 40))
              wall_dist[wall] = WALL_OPENING;
            else
              wall_dist[wall] = WALL_INVALID;
          }
        }
      }
    }
  }

  // Update public wall distance
  for (wall = 0; wall < NUM_WALLS; wall++) {
    if (wall_lock[wall]) {
      last_valid_dist[wall] = newDist[wall];
      wall_dist[wall] = newDist[wall];
    }
  }

  // Check alarm
  if ( ((wall_alarm_type == WALL_OPEN_LEFT) && (wall_dist[LEFT] == WALL_OPENING)) ||
       ((wall_alarm_type == WALL_OPEN_RIGHT) && (wall_dist[RIGHT] == WALL_OPENING)) ) {
    temp_callback = wall_callback;
    wall_callback = NULL;
    wall_alarm_type = WALL_NONE;
    if (temp_callback != NULL)
      temp_callback();
  }
}

/******************************************************************************/
void wall_alarm_set(int type, void (*func)(void))
{
  wall_alarm_type = type;
  wall_callback = func;
}

/******************************************************************************/
void wall_alarm_clr(void)
{
  wall_callback = NULL;
  wall_alarm_type = WALL_NONE;
}
